#include <signal.h>
#include <std_msgs/String.h>
#include "ros/ros.h"

#include <geometry_msgs/PointStamped.h>
#include <geometry_msgs/Twist.h>
#include <geometry_msgs/WrenchStamped.h>

#include <webots_ros/set_float.h>
#include <webots_ros/set_int.h>
#include <webots_ros/Int32Stamped.h>

#include <tf/transform_broadcaster.h>

#include <sensor_msgs/Imu.h>
#include <sensor_msgs/NavSatFix.h>
#include <math.h>
using namespace std;
#define TIME_STEP 30    //时钟
#define NMOTORS 2       //电机数量
#define MAX_SPEED 10.0   //电机最大速度

ros::NodeHandle *n;
string model_name="my_robot";
static int controllerCount;
static vector<string> controllerList; 
static double GPSvalues[3] = {0, 0, 0};
static double Inertialvalues[4] = {0, 0, 0, 0};
static const char *motorNames[NMOTORS] ={"motor1", "motor2"};
double pos[NMOTORS]={0,0};
double linear_temp ,angular_temp;
double l=0.18,d=0.08,r=0.025;


ros::ServiceClient timeStepClient;          //时钟通讯客户端
webots_ros::set_int timeStepSrv;            //时钟服务数据

ros::ServiceClient set_velocity_client;
webots_ros::set_float set_velocity_srv;

ros::ServiceClient set_position_client;   
webots_ros::set_float set_position_srv;  

/*******************************************************
* Function name ：controllerNameCallback
* Description   ：控制器名回调函数，获取当前ROS存在的机器人控制器
* Parameter     ：
        @ name   控制器名
* Return        ：无
**********************************************************/
void controllerNameCallback(const std_msgs::String::ConstPtr &name) { 
    controllerCount++; 
    controllerList.push_back(name->data);//将控制器名加入到列表中
    ROS_INFO("Controller #%d: %s.", controllerCount, controllerList.back().c_str());
}

/*******************************************************
* Function name ：broadcastTransform
* Description   ：tf函数
* Parameter     ：
* Return        ：无
**********************************************************/
void broadcastTransform()
{
  static tf::TransformBroadcaster br;
  tf::Transform transform;

  transform.setIdentity();
  br.sendTransform(tf::StampedTransform(transform,ros::Time::now(),"map","odom"));// 发送tf坐标关系

  transform.setOrigin(tf::Vector3(GPSvalues[0],GPSvalues[1],0));// 设置原点
  tf::Quaternion q(Inertialvalues[0],Inertialvalues[1],Inertialvalues[2],Inertialvalues[3]);// 四元数 ->欧拉角
//   q=q.inverse();
  transform.setRotation(q); //设置旋转数据
  br.sendTransform(tf::StampedTransform(transform,ros::Time::now(),"odom","base_link"));// 发送tf坐标关系
  
  transform.setOrigin(tf::Vector3(0,0,0));// 设置原点
  tf::Quaternion q1(tf::Vector3(1,0,0),M_PI);
  transform.setRotation(q1);
  br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "base_link", model_name+"/Sick_LMS_291"));
}
/*******************************************************
* Function name ：gpsCallback
* Description   ：GPS回调函数，获取GPS消息
* Parameter     ：
        @ values   GPS位置消息值
* Return        ：无
**********************************************************/
void gpsCallback(const geometry_msgs::PointStamped::ConstPtr &values)
{
    GPSvalues[0] = values->point.x;// x坐标
    GPSvalues[1] = values->point.y;// y坐标
    GPSvalues[2] = values->point.z;// z坐标
    broadcastTransform(); // tf坐标转换
}
/*******************************************************
* Function name ：inertial_unitCallback
* Description   ：IMU回调函数，获取IMU消息
* Parameter     ：
        @ values   IMU姿态消息值
* Return        ：无
**********************************************************/
void inertial_unitCallback(const sensor_msgs::Imu::ConstPtr &values)
{
    Inertialvalues[0] = values->orientation.x; 
    Inertialvalues[1] = values->orientation.y;
    Inertialvalues[2] = values->orientation.z;
    Inertialvalues[3] = values->orientation.w;
    broadcastTransform(); // tf坐标转换
}

/*******************************************************
* Function name ：updateSpeed
* Description   ：updateSpeed函数，update speed
* Parameter     ：None
* Return        ：无
**********************************************************/
void updateSpeed() {   
    
    pos[0]  = angular_temp*M_PI/2;
    pos[1]  += linear_temp;
    ROS_INFO("angular:%f,linear:%f",angular_temp*M_PI/2,linear_temp);
    set_position_client = n->serviceClient<webots_ros::set_float>(model_name+"/"+ string(motorNames[0]) + string("/set_position"));   
    set_position_srv.request.value = pos[0];
    set_position_client.call(set_position_srv);

    set_position_client = n->serviceClient<webots_ros::set_float>(model_name+"/"+ string(motorNames[1]) + string("/set_position"));   
    set_position_srv.request.value = pos[1];
    set_position_client.call(set_position_srv);
    
}
/*******************************************************
* Function name ：键盘返回函数
* Description   ：当键盘动作，就会进入此函数内
* Parameter     ：
        @value   返回的值
* Return        ：无
**********************************************************/
void keyboardDataCallback(const webots_ros::Int32Stamped::ConstPtr &value)
{
    switch (value->data)
    {
        case 314:
            angular_temp=-1;
            linear_temp=0.2;
            break;
        case 315:
            angular_temp=0;
            linear_temp =1;
            break;
        case 316:
            angular_temp =1;
            linear_temp=0.2;
            break;
        case 317:
            angular_temp =0;
            linear_temp=-1;
            break;
        case 32:
            linear_temp = 0;
            angular_temp = 0;
            break;
        default:
            break;
    }
    updateSpeed();
}

/*******************************************************
* Function name ：quit
* Description   ：退出函数
* Parameter     ：
        @sig   信号
* Return        ：无
**********************************************************/
void quit(int sig) {
    ROS_INFO("User stopped the '/robot' node.");
    timeStepSrv.request.value = 0; 
    timeStepClient.call(timeStepSrv); 
    ros::shutdown();
    exit(0);
}

int main(int argc, char **argv) {
    setlocale(LC_ALL, ""); // 用于显示中文字符
    string controllerName;
    // 在ROS网络中创建一个名为robot_init的节点
    ros::init(argc, argv, "my_robot", ros::init_options::AnonymousName);
    n = new ros::NodeHandle;
    // 截取退出信号
    signal(SIGINT, quit);

    // 订阅webots中所有可用的model_name
    ros::Subscriber nameSub = n->subscribe("model_name", 100, controllerNameCallback);
    while (controllerCount == 0 || controllerCount < nameSub.getNumPublishers()) {
        ros::spinOnce();
    }
    ros::spinOnce();
    // 服务订阅time_step和webots保持同步
    timeStepClient = n->serviceClient<webots_ros::set_int>(model_name+"/robot/time_step");
    timeStepSrv.request.value = TIME_STEP;

    // 如果在webots中有多个控制器的话，需要让用户选择一个控制器
    if (controllerCount == 1)
        controllerName = controllerList[0];
    else {
        int wantedController = 0;
        cout << "Choose the # of the controller you want to use:\n";
        cin >> wantedController;
        if (1 <= wantedController && wantedController <= controllerCount)
        controllerName = controllerList[wantedController - 1];
        else {
        ROS_ERROR("Invalid number for controller choice.");
        return 1;
        }
    }
    ROS_INFO("Using controller: '%s'", controllerName.c_str());
    // 退出主题，因为已经不重要了
    nameSub.shutdown();
    
    //GPS enable
    ros::ServiceClient gps_Client;          
    webots_ros::set_int gps_Srv;            
    ros::Subscriber gps_sub;
    gps_Client = n->serviceClient<webots_ros::set_int>(model_name+"/gps/enable"); // 使能GPS服务
    gps_Srv.request.value = TIME_STEP;
    // 判断gps使能服务是否成功
    if (gps_Client.call(gps_Srv) && gps_Srv.response.success) {
        ROS_INFO("gps enabled.");
        // 订阅gps话题，获取数据
        gps_sub = n->subscribe(model_name+"/gps/values", 1, gpsCallback);
        ROS_INFO("Topic for gps initialized.");
        while (gps_sub.getNumPublishers() == 0) {
        }
        ROS_INFO("Topic for gps connected.");
    } else {
        if (!gps_Srv.response.success)
        ROS_ERROR("Failed to enable gps.");
        return 1;
    }
    
    //IMU enable
    ros::ServiceClient inertial_unit_Client;          
    webots_ros::set_int inertial_unit_Srv;            
    ros::Subscriber inertial_unit_sub;
    inertial_unit_Client = n->serviceClient<webots_ros::set_int>(model_name+"/inertial_unit/enable"); //订阅IMU使能服务
    inertial_unit_Srv.request.value = TIME_STEP;
    // 判断是否使能成功
    if (inertial_unit_Client.call(inertial_unit_Srv) && inertial_unit_Srv.response.success) {
        ROS_INFO("inertial_unit enabled.");
        // 获取话题数据
        inertial_unit_sub = n->subscribe(model_name+"/inertial_unit/quaternion", 1, inertial_unitCallback);
        ROS_INFO("Topic for inertial_unit initialized.");
        while (inertial_unit_sub.getNumPublishers() == 0) {
        }
        ROS_INFO("Topic for inertial_unit connected.");
    } else {
        if (!inertial_unit_Srv.response.success)
        ROS_ERROR("Failed to enable inertial_unit.");
        return 1;
    }
    
    // for (int i = 0; i < NMOTORS; ++i) {
    //     // position速度控制时设置为0  
    //     set_position_client = n->serviceClient<webots_ros::set_float>(model_name +"/"+ string(motorNames[i]) + string("/set_position"));   
    //     set_position_srv.request.value = 0;
    //     if (set_position_client.call(set_position_srv) && set_position_srv.response.success)     
    //         ROS_INFO("Position set to 0 for motor %s.", motorNames[i]);   
    //     else     
    //         ROS_ERROR("Failed to call service set_position on motor %s.", motorNames[i]);
    //     // velocity初始速度设置为max_speed/2   
    //     set_velocity_client = n->serviceClient<webots_ros::set_float>(model_name +"/"+ string(motorNames[i]) + string("/set_velocity"));   
    //     set_velocity_srv.request.value = MAX_SPEED;   
    //     if (set_velocity_client.call(set_velocity_srv) && set_velocity_srv.response.success == 1)     
    //         ROS_INFO("Velocity set to max_speed/2 for motor %s.", motorNames[i]);   
    //     else     
    //         ROS_ERROR("Failed to call service set_velocity on motor %s.", motorNames[i]);
    // }   

    //lidar enable
    ros::ServiceClient lidar_Client;          
    webots_ros::set_int lidar_Srv;            
    lidar_Client = n->serviceClient<webots_ros::set_int>(model_name+"/Sick_LMS_291/enable"); // 订阅lidar使能服务
    lidar_Srv.request.value = TIME_STEP;
	// 判断是否使能成功
    if (lidar_Client.call(lidar_Srv) && lidar_Srv.response.success) {
	ROS_INFO("Sick_LMS_291 enabled.");
    } else {
    if (!lidar_Srv.response.success)
	ROS_ERROR("Failed to enable lidar.");
	return 1;
    }

    // 服务订阅键盘
    // ros::ServiceClient keyboardEnableClient;
    // webots_ros::set_int keyboardEnablesrv;
    // ros::Subscriber keyboardSub;
    // keyboardEnableClient = n->serviceClient<webots_ros::set_int>(model_name+"/keyboard/enable");
    // keyboardEnablesrv.request.value = TIME_STEP;
    // if (keyboardEnableClient.call(keyboardEnablesrv) && keyboardEnablesrv.response.success)
    // {
        
    //     keyboardSub = n->subscribe(model_name+"/keyboard/key",1,keyboardDataCallback);
    //     while (keyboardSub.getNumPublishers() == 0) {}
    //     setlocale(LC_CTYPE,"zh_CN.utf8");//设置中文
    //     ROS_INFO("Keyboard enabled.");
    //     ROS_INFO("控制方向：");
    //     ROS_INFO("  ↑  ");
    //     ROS_INFO("← ↓ →");
    //     ROS_INFO("刹车：空格键");
    //     ROS_INFO("Use the arrows in Webots window to move the robot.");
    //     ROS_INFO("Press the End key to stop the node.");
    // }
    // else
    // {
    //     ROS_ERROR("Could not enable keyboard, success = %d.", keyboardEnablesrv.response.success);
    //     return 1;
    // }


    // main loop
    while (ros::ok()) {
        if (!timeStepClient.call(timeStepSrv) || !timeStepSrv.response.success) {
        ROS_ERROR("Failed to call service time_step for next step.");
        
        // break;
        }
        // updateSpeed();
        ros::spinOnce();
        cout<<1<<endl;
    }
    
    timeStepSrv.request.value = 0;
    timeStepClient.call(timeStepSrv);
    ros::shutdown(); 
    return 0;
}
